/**
  ******************************************************************************
  * @file    cam.c
  * @author  Amaseing team
  * @version V1.0.0
  * @date    02-february-2013
  * @brief   Cam
  *         
  ******************************************************************************  
  */ 
  
/* Private includes ----------------------------------------------------------*/ 
#include "motor.h"

/* Private declarations ------------------------------------------------------*/
/* Private prototypes --------------------------------------------------------*/
volatile int time = 0;

/* Private defines -----------------------------------------------------------*/
#define MT_OFFSET 			(650)
#define MT_MULTIPLY			(4)

#define MT_SPEED 			(33) 		/* % */

#define MT_TIME_TURN90		(866) 	/* ms */
#define MT_TIME_TURN180		(1732) 	/* ms */
#define MT_TIME_TURN270		(2580) 	/* ms */
#define MT_TIME_TURN360		(3465) 	/* ms */

#define MT_TIME_DIST1CM		(147) 		/* ms */
#define MT_TIME_DIST2CM		(294) 		/* ms */
#define MT_TIME_DIST5CM		(735) 		/* ms */
#define MT_TIME_DIST10CM	(1470) 		/* ms */
#define MT_TIME_DIST15CM	(2200) 		/* ms */
/* Functions -----------------------------------------------------------------*/
void motorInit(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
	TIM_OCInitTypeDef TIM_OCInitStruct;
    
	// Init motor IO
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);

	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;

	GPIO_PinAFConfig(GPIOD, GPIO_PinSource12, GPIO_AF_TIM4);
	GPIO_PinAFConfig(GPIOD, GPIO_PinSource13, GPIO_AF_TIM4);
	GPIO_PinAFConfig(GPIOD, GPIO_PinSource14, GPIO_AF_TIM4);
	GPIO_PinAFConfig(GPIOD, GPIO_PinSource15, GPIO_AF_TIM4);

	GPIO_InitStructure.GPIO_Pin = (GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15);
	GPIO_Init(GPIOD, &GPIO_InitStructure);
    
	//Init PWM timer	
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
	
	//Init Time base unit
	
	//Timer clock TIM4 = 84MHz.
	//TIM_CKD_DIV1 		-> 	84 / 3360 = 25kHz
	
	TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInitStruct.TIM_Period = 3360; // = 25kHz.
	TIM_TimeBaseInitStruct.TIM_Prescaler = 0;
	TIM_TimeBaseInit(TIM4, &TIM_TimeBaseInitStruct);
	
	//Init channels 1-4
	TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStruct.TIM_Pulse = 0;//336; //Duty cycle
	TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High;
	
	TIM_OC1Init(TIM4, &TIM_OCInitStruct); 
	TIM_OC2Init(TIM4, &TIM_OCInitStruct);
	TIM_OC3Init(TIM4, &TIM_OCInitStruct);
	TIM_OC4Init(TIM4, &TIM_OCInitStruct);
	
	TIM_Cmd(TIM4, ENABLE);
	
	//Init counter timer
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
	
	//Timer clock TIM2 = 84MHz.
	//TIM_CKD_DIV1 		-> 	84 / 8400 = 10kHz
	//10kHz / 10 		[=period]	= 1000Hz. (1ms)
	//10kHz / 100 	[=period]	= 100Hz. 	(10ms)
	//10kHz / 1000	[=period]	= 10Hz. 	(100ms)
	
	TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV4;
	TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInitStruct.TIM_Period = 10;
	TIM_TimeBaseInitStruct.TIM_Prescaler = 8400;
	
	TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStruct);
	
	TIM_ClearFlag(TIM2, TIM_IT_Update);
	TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
	
	NVIC_InitTypeDef NVIC_InitStructure;

  // Enable the TIM2_IRQn Interrupt
  NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  NVIC_Init(&NVIC_InitStructure);
	
	NVIC_EnableIRQ(TIM2_IRQn);
	
	TIM_Cmd(TIM2, ENABLE);
}

void motorCirleTurn(enum eTURN turn)
{
	//
}
void motorPointTurn(enum eTURN turn)
{
	if(turn == TURN90L)
		motorSetSpeed(-MT_SPEED,MT_SPEED,MT_TIME_TURN90);
	if(turn == TURN90R)
		motorSetSpeed(MT_SPEED,-MT_SPEED,MT_TIME_TURN90);
	if(turn == TURN180L)
		motorSetSpeed(-MT_SPEED,MT_SPEED,MT_TIME_TURN180);
	if(turn == TURN180R)
		motorSetSpeed(MT_SPEED,-MT_SPEED,MT_TIME_TURN180);
	if(turn == TURN270L)
		motorSetSpeed(-MT_SPEED,MT_SPEED,MT_TIME_TURN270);
	if(turn == TURN270R)
		motorSetSpeed(MT_SPEED,-MT_SPEED,MT_TIME_TURN270);
	if(turn == TURN360L)
		motorSetSpeed(-MT_SPEED,MT_SPEED,MT_TIME_TURN360);
	if(turn == TURN360R)
		motorSetSpeed(MT_SPEED,-MT_SPEED,MT_TIME_TURN360);
}
void motorDriveDistance(enum eDIRECTION dir, enum eDISTANCE dist)
{
	if(dir == FORWARD){
		if(dist == DIST1CM)
			motorSetSpeed(MT_SPEED,MT_SPEED,MT_TIME_DIST1CM);
		if(dist == DIST2CM)
			motorSetSpeed(MT_SPEED,MT_SPEED,MT_TIME_DIST2CM);
		if(dist == DIST5CM)
			motorSetSpeed(MT_SPEED,MT_SPEED,MT_TIME_DIST5CM);
		if(dist == DIST10CM)
			motorSetSpeed(MT_SPEED,MT_SPEED,MT_TIME_DIST10CM);
		if(dist == DIST15CM)
			motorSetSpeed(MT_SPEED,MT_SPEED,MT_TIME_DIST15CM);
	}
	if(dir == BACKWARD){
		if(dist == DIST1CM)
			motorSetSpeed(-MT_SPEED,-MT_SPEED,MT_TIME_DIST1CM);
		if(dist == DIST2CM)
			motorSetSpeed(-MT_SPEED,-MT_SPEED,MT_TIME_DIST2CM);
		if(dist == DIST5CM)
			motorSetSpeed(-MT_SPEED,-MT_SPEED,MT_TIME_DIST5CM);
		if(dist == DIST10CM)
			motorSetSpeed(-MT_SPEED,-MT_SPEED,MT_TIME_DIST10CM);
		if(dist == DIST15CM)
			motorSetSpeed(-MT_SPEED,-MT_SPEED,MT_TIME_DIST15CM);
	}
}
void motorSetSpeed(int left, int right, int ms)
{
	TIM_Cmd(TIM4, DISABLE);
	
	if(left < -100)
		return;
	if(left > 100)
		return;
	if(right < -100)
		return;
	if(right > 100)
		return;
	if(ms < 0)
		return;
	if(ms > 10000)
		return;

	if(left < 0)
	{
		left *= -1;
		left *= MT_MULTIPLY;
		left += MT_OFFSET;
		TIM4->CCR1 = left; 	//FWD Motor A
		TIM4->CCR2 = 0;			//REV Motor A
	}
	else if (left == 0)
	{
		TIM4->CCR1 = 0; 		//FWD Motor A
		TIM4->CCR2 = 0;			//REV Motor A
	}
	else
	{
		left *= MT_MULTIPLY;
		left += MT_OFFSET;
		TIM4->CCR1 = 0; 		//FWD Motor A
		TIM4->CCR2 = left;	//REV Motor A
	}

	if(right < 0)
	{
		right *= -1;
		right *= MT_MULTIPLY;
		right += MT_OFFSET;
		TIM4->CCR3 = right; //FWD Motor A
		TIM4->CCR4 = 0;			//REV Motor A
	}
	else if (right == 0)
	{
		TIM4->CCR3 = 0; 		//FWD Motor A
		TIM4->CCR4 = 0;			//REV Motor A
	}
	else
	{
		right *= MT_MULTIPLY;
		right += MT_OFFSET;
		TIM4->CCR3 = 0; 		//FWD Motor A
		TIM4->CCR4 = right;	//REV Motor A
	}
	
	time = ms;

	TIM_Cmd(TIM4, ENABLE);
	TIM_Cmd(TIM2,	ENABLE);
}
void motorBrake(void)
{
	time = 0;
	TIM_Cmd(TIM2,DISABLE);
	
	//Stop motors
	TIM4->CCR1 = 0;
	TIM4->CCR2 = 0;
	TIM4->CCR3 = 0;
	TIM4->CCR4 = 0;
}
void motorWaitWhileDriving(void)
{
	volatile uint32_t counter;
	do {
		counter = 10000;
		while (counter--);
	}
	while(TIM2->CR1&0x0001);
}
uint32_t motorDrivingDone(void)
{
	if(TIM2->CR1&0x0001)
		return 0;
	return 1;
}

void TIM2_IRQHandler(void)
{
	if(TIM_GetITStatus(TIM2, TIM_IT_Update))
	{
		if(time > 0)
		{
			time--;
		}
		else
		{
			TIM_Cmd(TIM2,DISABLE);
			
			//Stop motors
			TIM4->CCR1 = 0;
			TIM4->CCR2 = 0;
			TIM4->CCR3 = 0;
			TIM4->CCR4 = 0;
		}
		//TIM_ClearITPendingBit(TIM2,TIM_IT_Update);
		TIM_ClearFlag(TIM2, TIM_IT_Update);
	}
}




